/*
 * Copyright (c) 2006-2021, Halo Team
 *
 * SPDX-License-Identifier: GPLv3
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-26     Eywen       the first version
 */


#include "gcode.h"
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include "../common.h"
#include "../fdm_printer.h"
#include "gcode_head.h"
//#include "gcode_file.h"

typedef struct gcode_str{
    char *str;
    uint8_t read_idx;
    uint8_t len;
}gcode_str_t;

/* gcode处理接口 */
const static gcode_handle_map_t s_gcode_handle_map_list[] = {
    {{'G', "0"}, gcode_handle_G0_G1},
    {{'G', "1"}, gcode_handle_G0_G1},
    {{'M', "23"}, gcode_handle_M23}
};

static inline bool gcode_str_init(gcode_str_t *gcode_str, char *str)
{
    ASSERT_NULL(gcode_str);
    ASSERT_NULL(str);

    gcode_str->str = str;
    gcode_str->read_idx = 0;
    gcode_str->len = strlen(str);

    return true;
}

/* 从字符串中取出一个非空白的字符,遇到空白字符结束 */
static inline bool get_char_skip_space(gcode_str_t *gcode_str, uint8_t *byte)
{
    uint8_t tmp;
    while(gcode_str->read_idx < gcode_str->len){
        tmp = *(gcode_str->str + gcode_str->read_idx);
        gcode_str->read_idx++;
        /* 不为空格返回0 */
        if(0 == isspace(tmp)){
            *byte = tmp;
            return true;
        }
    }
    return false;
}

/* 移除前面的空白字符 */
static inline bool rm_gcode_pre_space(gcode_str_t *gcode_str)
{
    while(gcode_str->read_idx < gcode_str->len){
        if(0 == isspace(*(gcode_str->str + gcode_str->read_idx))){
            return true;
        }
        else{
            gcode_str->read_idx++;
        }
    }
    return false;
}

/* 获取一个单词, 跳过前面的空格，遇到空白字符停止 */
static bool get_word_skip_space(gcode_str_t *gcode_str, char *str)
{
    ASSERT_NULL(gcode_str);
    ASSERT_NULL(str);

    uint8_t tmp;
    bool flag = false;
    if(false == rm_gcode_pre_space(gcode_str)){
        return false;
    }
    while(gcode_str->read_idx < gcode_str->len){
            tmp = *(gcode_str->str + gcode_str->read_idx);
            gcode_str->read_idx++;
            /* 不为空白字符  */
            if(0 == isspace(tmp)){
                *(str++) = tmp;
                flag = true;
            }
            else{
                *str = '\0';
                return flag;
            }
    }
    *str = '\0';
    return flag;
}

bool parse_gcode_str(gcode_t *gcode, char *str)
{
    ASSERT_NULL(gcode);
    ASSERT_NULL(str);

    gcode_str_t gcode_str;
    gcode_str_init(&gcode_str, str);

    /* 参数个数 */
    gcode->para_cnt = 0;

    /* 跳过命令前面的空格获取命令名 */
    if(false == get_char_skip_space(&gcode_str, &(gcode->comd.comd))){
        /* 表示gcode解析不完整  */
        gcode->para_cnt = -1;
        return false;
    }

    /* 获取命令名号 */
    if(false == get_word_skip_space(&gcode_str, gcode->comd.order)){
        /* 表示gcode解析不完整  */
        gcode->para_cnt = -1;

        return false;
    }

    uint8_t head;
    /* 每次跳过前面的空格 */
    while(get_char_skip_space(&gcode_str, &head) == true){
        gcode->para[gcode->para_cnt].head = head;
        /* 提取出一个单词（数字）,空格结束  */
        if(true == get_word_skip_space(&gcode_str, gcode->para[gcode->para_cnt].number)){
            gcode->para_cnt++;
        }
        else{
            if(gcode_str.read_idx == gcode_str.len){
                return true;
            }
            /* 表示gcode解析不完整  */
            gcode->para_cnt = -1;
            return false;
        }
    }
    return true;
}

static bool gcode_handle_task_empty(gcode_handle_t *handle)
{
    ASSERT_NULL(handle);
    return ringbuff_is_empty(handle->handle_list);
}

static bool gcode_handle_task_pop(gcode_handle_t *handle, gcode_t *gcode)
{
    ASSERT_NULL(handle);
    ASSERT_NULL(gcode);
    return ringbuff_pop(handle->handle_list, gcode);
}

static bool gcode_handle_task_run(gcode_handle_t *handle)
{
    ASSERT_NULL(handle);
    debug_info("gcode handle task running...");
    int i;
    int gcode_handle_cnt;
    gcode_t gcode;
    /* 获取gcode处理list的长度 */
    gcode_handle_cnt = sizeof(s_gcode_handle_map_list)/sizeof(gcode_handle_map_t);

    for(;;){
        /* 如果handle list里面为空，则放弃进程 */
        while(gcode_handle_task_empty(handle) == true){
            //debug_info("gcode_t task empty");
            rt_thread_yield();
        }
        do{
            memset(&gcode, 0, sizeof(gcode));
            //debug_info("ringbuff_used_count: %d", ringbuff_used_count(handle->handle_list));
            /* 从任务队列中取出一个任务  */
            gcode_handle_task_pop(handle, &gcode);
            /* 线性查找处理函数  */
            for(i = 0; i < gcode_handle_cnt; i++){

//                debug_info("cmp: %c %s %c %s", s_gcode_handle_map_list[i].comd.comd,
//                        s_gcode_handle_map_list[i].comd.order,
//                        gcode.comd.comd,
//                        gcode.comd.order);

//                /* 对比命令名和命令序号 */
                if((s_gcode_handle_map_list[i].comd.comd == gcode.comd.comd) &&
                        (strcmp(s_gcode_handle_map_list[i].comd.order, gcode.comd.order) == 0)){
                    /* 查找到相应的处理接口  */
                    if(s_gcode_handle_map_list[i].handle != NULL){
                        s_gcode_handle_map_list[i].handle(&gcode,
                                false, handle->parent);
                    }
                    else{
                        debug_info("gcode handle is null %c %s",
                                gcode.comd.comd, gcode.comd.order);
                    }
                    /* 解析完毕，退出查找，继续下一条gcode的解析和执行 */
                    break;
                }
            }
        /* 不为空则作死的执行gcode任务 */
        }while(gcode_handle_task_empty(handle) == false);
    }
    return false;
}

static bool gcode_handle_task_start(gcode_handle_t *handle)
{
    ASSERT_NULL(handle);
    debug_info("gcode handle task try start");
    /* 线程创建成功，开始运行 */
    if(handle->gcode_handle_thread_id != NULL){
        return (rt_thread_startup(handle->gcode_handle_thread_id) == RT_EOK);
    }
    debug_error("gcode task start fail");
    return false;
}

static bool gcode_handle_init(gcode_handle_t *handle)
{
    ASSERT_NULL(handle);
    ASSERT_NULL(handle->ops);

    /* 创建gcode处理线程 */
    handle->gcode_handle_thread_id = rt_thread_create("gcode",
            (void(*)(void *parameter))gcode_handle_task_run, handle,
            GCODE_HANDLE_THREAD_STACK_SIZE,
            GCODE_HANDLE_THREAD_PRIORITY,
            GCODE_HANDLE_THREAD_TIMESLICE);

    if(handle->gcode_handle_thread_id != RT_NULL){
        //debug_info("gcode handle init success");
        return true;
    }

    debug_error("gcode handle init fail");
    return false;
}

static bool gcode_handle_task_full(gcode_handle_t *handle)
{
    ASSERT_NULL(handle);
    return ringbuff_is_full(handle->handle_list);
}

static bool gcode_handle_add_gcode_task(gcode_handle_t *handle, gcode_t *gcode)
{
    ASSERT_NULL(handle);
    ASSERT_NULL(gcode);

    return ringbuff_push(handle->handle_list, gcode);
}

static void gcode_handle_set_parent(gcode_handle_t *handle, void *parent)
{
    ASSERT_NULL(handle);

    handle->parent = parent;
}

const static gcode_handle_ops_t s_gcode_handle_ops = {
    .init = gcode_handle_init,
    .task_empty = gcode_handle_task_empty,
    .task_full = gcode_handle_task_full,
    .pop_task = gcode_handle_task_pop,
    .start = gcode_handle_task_start,
    .add_gcode = gcode_handle_add_gcode_task,
    .set_parent = gcode_handle_set_parent,
};

typedef struct test_gcode{
    int x;
    int y;
    int z;
    int e;
    int speed;
}test_gcode_t;

//#if 1
//const static test_gcode_t test_gcode[] = {
//    {0  ,   0,      0,       0,  4000},
//    {50000, 5000,   5000,    0,  4000},
//    {50000, 5000,   0,       0,  4000},
//    {10000,   0,    0,       0,  4000},
//    {15000,5000,    0,       0,  4000},
//    {15000,5000,    5000,    0,  4000},
//    {20000,   0,    5000,    0,  4000},
//    {25000,5000,    5000,    0,  4000},
//};
//#else
//const static test_gcode_t test_gcode[] = {
//    {0   ,    0,    0,    100, 4000},
//    {0,    5000,    0,    0, 4000},
//    {5000, 2500,    0,    100, 4000},
//    //{2000   , 5000,    0,    0, 1500},
//};
//#endif

void gcode_test(gcode_handle_t *handle)
{
    char *str;
    extern gcode_handle_t gcode_handle;
    gcode_t gcode;
//
//    for(int i = 0; i < sizeof(gcode_exp1)/sizeof(char *); i++){
////        snprintf(str, sizeof(str) - 1, "G0 X%d Y%d Z%d E%d F%d", test_gcode[i].x, test_gcode[i].y,
////                test_gcode[i].z, test_gcode[i].e, test_gcode[i].speed);
//        //debug_info("gcode: %s", str);
//        str = (char *)gcode_exp1[i];
//        if(true == parse_gcode_str(&gcode, str)){
//            //continue;
//            while(gcode_handle.ops->task_full(&gcode_handle) == true){
//                rt_thread_yield();
//            }
//
//            gcode_handle.ops->add_gcode(&gcode_handle, &gcode);
////            debug_info("gcode: %s ringbuff_used_count: %d ringbuff_is_empty:%d", str,
////                    ringbuff_used_count(gcode_handle.handle_list),
////                    ringbuff_is_empty(gcode_handle.handle_list));
//        }
//    }
}

/* 注册gcode handle对象 ，申请必要的内存空间 */
bool gcode_handle_register(gcode_handle_t *device)
{
    ASSERT_NULL(device);

    MEM_MEMSET(device, 0, sizeof(gcode_handle_t));

    device->ops = &(s_gcode_handle_ops);

    /* 创建ringbuff标识空间 */
    device->handle_list = MEM_MALLOC(sizeof(ringbuff_t));
    if(device->handle_list == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    int ringbuff_size;
    void *ptr;
    ringbuff_size = GCODE_RINGBUFF_LEN*sizeof(gcode_t);
    /* 申请存储ringbuff内容的空间 */
    ptr = MEM_MALLOC(ringbuff_size);
    if(ptr == NULL){
        debug_error("Insufficient system memory");
        goto err;
    }

    /* 初始化ringbuff，绑定空间与ringbuff变量 */
    ringbuff_init(device->handle_list, ptr, ringbuff_size, sizeof(gcode_t));

    debug_info("gcode handle register success");
    return true;
err:
    if(NULL != device->handle_list){
        if(NULL != device->handle_list->addr){
            ringbuff_destroy(device->handle_list);
        }
        MEM_FREE(device->handle_list);
        device->handle_list = NULL;
    }

    return false;
}


inline float str_to_float(char *str)
{
#define IS_DIGIT(c)    (((c) <= '9') && ((c) >= '0'))
#define CHARACTER_TO_DIGIT(c)   ((c) - '0')
#define IS_DOT(c)      ((c) == '.')
#define IS_NEGTIVE(c)     ((c) == '-')
/* 10的n次方                                         0   1   2    3     4      5       6        7         8     */
const static int power_10[9] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000};
#define MATH_POWER(n)          (power_10[n])

    ASSERT_NULL(str);
    int tmp_integer;
    int decimal;
    int decimal_cnt;
    tmp_integer = decimal = 0;
    decimal_cnt = 0;
    int neg = 1;

    if(IS_NEGTIVE(*str)){
        neg = -1;
        str++;
    }

    while(IS_DIGIT(*str)){
        tmp_integer *= 10;
        tmp_integer += CHARACTER_TO_DIGIT(*str);
        str++;
    }
    if(IS_DOT(*str)){
        str++;
        while(IS_DIGIT(*str)){
            decimal *= 10;
            decimal += CHARACTER_TO_DIGIT(*str);
            str++;
            decimal_cnt++;
        }
    }
    return neg*((float)tmp_integer + (float)decimal*1.0/MATH_POWER(decimal_cnt));

#undef IS_DIGIT
#undef CHARACTER_TO_DIGIT
#undef IS_DOT
#undef MATH_POWER
}
